% -------------------------------------------------------------------------
% plot transfer orbit
%
% plot_orbit(p0_new , auxdata)
%
% Input argumuents:
% -------------------------------------------------------------------------
% p0           [14x1 double]     state and costate variables      [ - ]        
% auxdata      [structure]       parameters                       [ - ]
%
% Output argumuents:
% -------------------------------------------------------------------------
% None
%
% External functions called:
% -------------------------------------------------------------------------
% fx.m
% fy.m
% Sw_.m
%
% Copyright(C) 2015/06/22 by Chen Zhang, 
% School of Astronautics, Beihang University
% chenzhang.buaa@gmail.com
% -------------------------------------------------------------------------

function plot_orbit(p0 , auxdata)
mu = auxdata.mu;
Tmax = auxdata.Tmax;
c = auxdata.c;
LU = auxdata.LU;
TU = auxdata.TU;
VU = auxdata.VU;
MU = auxdata.MU;
x0 = auxdata.x0;
xf = auxdata.xf;

% plot figure 1
figure('position' , [100 , 100 , 600 , 500])
% plot initial orbit under rotating frame
options = odeset('RelTol',1e-12,'AbsTol', 1e-12);
[tt0 , xx0] = ode113(@fx , [0 , 39000 / TU] , [x0(1:6)] , options , auxdata);
plot3(xx0(: , 1) , xx0(: , 2) , xx0(: , 3) , 'k' , 'LineWidth' , 2); hold on;
% plot Halo orbit under rotating frame
[ttf , xxf] = ode113(@fx , [0 , 1.1e6 / TU] , [xf(1:6)], options , auxdata);
plot3(xxf(: , 1) , xxf(: , 2) , xxf(: , 3) , 'm' , 'LineWidth' , 2); hold on;
% plot transfer orbit under rotating frame
[tt , xx] = ode113(@fy , [0 , p0(end)] , [auxdata.x0 ; p0(1 : 7)] , ...
    options , auxdata);
plot3(xx(: , 1) , xx(: , 2) , xx(: , 3) , 'r' , 'LineWidth' , 2); hold on;
% plot Earth and Moon under rotating frame
em_rot;

% plot figure 2
figure('position' , [150 , 150 , 600 , 500])
% plot transfer orbit under Earth centered inertial frame
[XX , TT] = rot2ini(xx(: , 1:6) , tt  ,mu);
h1 = plot3(XX(: , 1) , XX(: , 2) , XX(: , 3) , 'r' , ...
    'LineWidth' , 2); hold on;
% plot Halo orbit under Earth centered inertial frame
[XXf , TTf] = rot2ini(xxf(: , 1:6) , ttf + tt(end)  ,mu);
h2 = plot3(XXf(: , 1) , XXf(: , 2) , XXf(: , 3) , 'm' ,...
    'LineWidth' , 2); hold on;
% mark transfer orbit and Halo orbit
% legend([h1, h2] , 'Thrust arc' , 'Halo orbit');
% plot Earth and Moon under Earth centered inertial frame
em_ini;
view(0 , 90);

% plot figure 3
figure('position' , [200 , 200 , 600 , 500])
Sk_array = zeros(size(tt));
u_array = zeros(size(tt));
H_array = zeros(size(tt));
for k = 1 : size(xx , 1)
    x1 = xx(k , 1); x2 = xx(k , 2); x3 = xx(k , 3); x4 = xx(k , 4); 
    x5 = xx(k , 5); x6 = xx(k , 6); x7 = xx(k , 7);
    x8 = xx(k , 8); x9 = xx(k , 9); x10 = xx(k , 10); x11 = xx(k , 11); 
    x12 = xx(k , 12); x13 = xx(k , 13); x14 = xx(k , 14);
    % calculate switching function
    Sk = -(x11^2 + x12^2 + x13^2)^(1/2) * auxdata.c / x7 - x14; 
    if Sk < 0
        u = 1;
    else
        u = 0;
    end
    u_array(k) = u;
    Sk_array(k) = Sk;
    H_array(k) = x10*x6 - x13*((mu*x3)/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) -...
        (x3*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) + ...
        (Tmax*u*x13)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) + x4*x8 + x5*x9 - ...
        x12*(2*x4 - x2 + (mu*x2)/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) - ...
        (x2*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) + ...
        (Tmax*u*x12)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) + x11*(x1 + 2*x5 - ...
        (mu*(mu + x1 - 1))/((mu + x1 - 1)^2 + x2^2 + x3^2)^(3/2) + ...
        ((mu + x1)*(mu - 1))/((mu + x1)^2 + x2^2 + x3^2)^(3/2) - ...
        (Tmax*u*x11)/(x7*(x11^2 + x12^2 + x13^2)^(1/2))) - (Tmax*u*x14)/c + 1;
end
subplot(4,1,1)
plot(tt , u_array , 'r' , 'LineWidth' , 2); hold on;
ylabel('u');
subplot(4,1,2)
plot(tt , Sk_array , 'b' , 'LineWidth' , 2); hold on;
ylabel('S');
subplot(4,1,3)
plot(tt , xx(: , 7) , 'b' , 'LineWidth' , 2); hold on;
ylabel('m');
subplot(4,1,4)
plot(tt , H_array , 'b' , 'LineWidth' , 2); hold on;
ylabel('Hamiltonian');
xlabel('time(TU)');
end
% -------------------------------------------------------------------------
